#include <Algorithm/projective_recon.h>
#include <Mesh/libcell_iterator.h>
#include <Matrix/LB_Matrix.h>
#include <Math/LB_Quaternions.h>
#define quote lib_cell_quote

typedef struct Greedy_Projection_Vert_Prop{
    double normal[3];
    double p[3];
    double theta;
    int is_legal; 
    int is_init; 
    int active;

}Greedy_Projection_Vert_Prop;
static inline void greedy_projection_vert_prop_init(Greedy_Projection_Vert_Prop* gpvp)
{
    memset(gpvp->normal,0,sizeof(double)*3);
    memset(gpvp->p,0,sizeof(double)*3);

    gpvp->is_legal=0;
    gpvp->theta=0;
    gpvp->is_init=1;
    gpvp->active=0;

}

static inline double* compute_triangle_normal(double* q1,double* q2,double* q3)
{
    double p1[3]={q2[0]-q1[0],q2[1]-q1[1],
        q2[2]-q1[2]};
    double p2[3]={q3[0]-q1[0],q3[1]-q1[1],
        q3[2]-q1[2]};
    double*re=out_product(p1,p2); 
    normalize(re,3);
    return re;
}


//***************
//大于0是在外心圆内部
//小于0是在外心圆外部
//****************
static inline double is_concyclic_four_vertices(double* p1,double* p2,double* p3,double* p4)
{
    LB_Matrix* mat=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(mat,4,4); 
    double* data=(double*)(mat->data);
    data[0*4+0]=p1[0]; data[0*4+1]=p1[1];data[0*4+2]=p1[0]*p1[0]+p1[1]*p1[1];data[0*4+3]=1.0;
    data[1*4+0]=p2[0]; data[1*4+1]=p2[1];data[1*4+2]=p2[0]*p2[0]+p2[1]*p2[1];data[1*4+3]=1.0;
    data[2*4+0]=p3[0]; data[2*4+1]=p3[1];data[2*4+2]=p3[0]*p3[0]+p3[1]*p3[1];data[2*4+3]=1.0;
    data[3*4+0]=p4[0]; data[3*4+1]=p4[1];data[3*4+2]=p4[0]*p4[0]+p4[1]*p4[1];data[3*4+3]=1.0;
    double *re=((double*)(mat->det(mat)));
    double re1=*re;
    lb_matrix_free(mat);
    free(re);
    return re1;
}


static inline double compute_triangle_area(double* p1,double* p2,double* p3)
{
    double q1[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};
    
    double q2[3]={p3[0]-p1[0],p3[1]-p1[1],p3[2]-p1[2]};

    double q[3]={q1[1]*q2[2]-q1[2]*q2[1],-q1[0]*q2[2]+q1[2]*q2[0],q1[0]*q2[1]-q1[1]*q2[0]};
    double re=SAFE_SQRT(q[0]*q[0]+q[1]*q[1]+q[2]*q[2])/2.0;

    return re;
}

static inline double compute_score_triangle(template_v* v1,template_v* v2,template_v* v3 )
{
    double normal[3]={0};
    Greedy_Projection_Vert_Prop*gpvp=(Greedy_Projection_Vert_Prop*)(v1->prop);
    normal[0]+=1.0*gpvp->normal[0]; normal[1]+=1.0*gpvp->normal[1];normal[2]+=1.0*gpvp->normal[2];
    gpvp=(Greedy_Projection_Vert_Prop*)(v2->prop); 
    normal[0]+=gpvp->normal[0]; normal[1]+=gpvp->normal[1];normal[2]+=gpvp->normal[2];
    gpvp=(Greedy_Projection_Vert_Prop*)(v3->prop); 
    normal[0]+=gpvp->normal[0]; normal[1]+=gpvp->normal[1];normal[2]+=gpvp->normal[2]; 
    normalize(normal,3);

    double* n1=compute_triangle_normal(v1->point,v2->point,v3->point );

    //double re=n1[0]*gpvp->normal[0]+n1[1]*gpvp->normal[1]+n1[2]*gpvp->normal[2]+1.0 ;
    double re=n1[0]*normal[0]+n1[1]*normal[1]+n1[2]*normal[2];

    SAFE_FREE(n1);

    return re;
}

static inline double compute_weight_score_triangle(template_v* v1,template_v* v2,template_v* v3)
{
    double score=compute_score_triangle(v1,v2,v3);
    double weight=compute_triangle_area(v1->point,v2->point,v3->point);
    return score*weight;
}

static inline double compute_weight_score_2triangles(template_v* v1,template_v* v2,template_v* v3,
    template_v* v4,template_v* v5,template_v* v6 )
{
    double score1=compute_score_triangle(v1,v2,v3);
    double weight1=compute_triangle_area(v1->point,v2->point,v3->point);
    double score2=compute_score_triangle(v4,v5,v6);
    double weight2=compute_triangle_area(v4->point,v5->point,v6->point);
 
    return (score1*weight1+score2*weight2)/(weight1+weight2);
}

static inline  template_f* swap_edge(Mesh* m, template_f* f,template_v** vs)
{
    printf("begin swap\n");
    template_c* c1=f->halffaces[0]->cell;
    template_c* c2=f->halffaces[1]->cell; 
    m->delete_cell(m,*c1,true);
    m->delete_cell(m,*c2,true);
    
    template_v* vs1[3]={vs[3],vs[0],vs[1]};
    m->create_cellv(m,vs1,3);
    vs1[0]=vs[1];vs1[1]=vs[2];vs1[2]=vs[3];
    m->create_cellv(m,vs1,3);

    template_v* vs2[2]={vs[1],vs[3]};
    template_f* re= m->get_facev(m,vs2,2);
     
    return re; 
}

static template_f* touying_tiaozheng_edge(Mesh* m, template_f* f)
{
   
    template_v* vs[4]={0};

    template_c* c1=f->halffaces[0]->cell;
    template_c* c2=f->halffaces[1]->cell;
 
    vs[0]=f->halffaces[0]->vertices[0]; vs[2]= f->halffaces[0]->vertices[1];
    printf("once tiaozhengherer1 %d %d\n",c1->id,c2->id);
    printf("once tiaozhengherer2 %d %d %d %d %d %d\n",c1->vertices[0]->id,c1->vertices[1]->id,c1->vertices[2]->id,
        c2->vertices[0]->id,c2->vertices[1]->id,c2->vertices[2]->id);

    for(int i=0;i<3;i++)
    {
        template_v* v=c1->vertices[i];

        if(v!=vs[0]&&v!=vs[2])
        {
            vs[3]=v;
            break;
        }
    }  
    for(int i=0;i<3;i++)
    {
        template_v* v=c2->vertices[i];
        if(v!=vs[0]&&v!=vs[2])
        {
            vs[1]=v;
            break;
        }
    } 
    printf("once tiaozhengherer2 %d %d %d %d %d %d\n",c1->vertices[0]->id,c1->vertices[1]->id,c1->vertices[2]->id,
        c2->vertices[0]->id,c2->vertices[1]->id,c2->vertices[2]->id);

    Greedy_Projection_Vert_Prop*gpvps[4]={0};
    gpvps[0]=(Greedy_Projection_Vert_Prop*)(vs[0]->prop);
    gpvps[1]=(Greedy_Projection_Vert_Prop*)(vs[1]->prop);
    gpvps[2]=(Greedy_Projection_Vert_Prop*)(vs[2]->prop);
    gpvps[3]=(Greedy_Projection_Vert_Prop*)(vs[3]->prop);
    printf("is is_concyclic_four_vertices:%lf\n",
        is_concyclic_four_vertices(gpvps[0]->p,gpvps[1]->p,gpvps[2]->p,gpvps[3]->p));

    printf("%d %d %d %d\n",vs[0]->id,vs[1]->id,vs[2]->id,vs[3]->id);

    if(is_concyclic_four_vertices(gpvps[0]->p,gpvps[1]->p,gpvps[2]->p,gpvps[3]->p)<=0)
    {
        return NULL;
    }
  
    double score1=compute_weight_score_2triangles(vs[1],vs[2],vs[0],
        vs[3],vs[0],vs[2]);

    double score2=compute_weight_score_2triangles(vs[0],vs[1],vs[3],
        vs[2],vs[3],vs[1]);
    
    printf("score1:%lf score2:%lf\n",score1,score2);
    if(score2>=0.97*score1)
    {
        return swap_edge(m,f,vs);
    } 
   
    return NULL;
}